#include "OI.h"
#include "Robotmap.h"
#include "Commands/Drivebackandforth.h"
#include "Commands/CountPress.h"



OI::OI() {
	rightStick = new Joystick(RIGHTSTICK);
	leftStick = new Joystick(LEFTSTICK);
	m_station = DriverStation::GetInstance();
	m_screen =  DriverStationLCD::GetInstance();
#ifdef SMARTDASHBOARD
	m_smartDashboard = SmartDashboard::GetInstance();
#endif
	m_screen->PrintfLine(DriverStationLCD::kUser_Line1, "Hi");
	m_screen->UpdateLCD();
	rightTrigger = new JoystickButton(rightStick, Joystick::kTopButton);
	rightTrigger->WhenPressed(new DriveBackAndForth());
	leftTrigger = new JoystickButton(leftStick, Joystick::kTopButton);
	leftTrigger->WhenPressed(new CountPress());
	}

float OI::getRightStickY() {
	float ret;
	ret = rightStick->GetY();
	return ret;
}

float OI::getRightStickX() {
	float ret;
	ret = rightStick->GetX();
	return ret;
}

float OI::getLeftStickY(){
	float ret;
	ret = leftStick->GetY();
	return ret;
}

float OI::getLeftStickX(){
	float ret;
	ret = leftStick->GetX();
	return ret;
}

float OI::getCrouch(){
	float crouch;
	crouch = rightStick->GetZ();
	crouch = 1-crouch;
	crouch = crouch/2;
	return crouch;
}
